#ifndef _MMPC_H_
#define _MMPC_H_

#include <ros/ros.h>
#include <Eigen/Eigen>
#include "tube_planning/tube_planning.h"
#include <Eigen/Eigen>
#include "tube_planning/polynomial.h"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include "tube_planning/config.h"

namespace Tube_planning
{
    class mmpc
    {
        private:
        polyCurve traj_info;
        struct MPCVar
        {
            int N; // predict horzion
            double Ts; // sample time
            // state input bound
            Eigen::MatrixXd bound;
            // cost parameter
            double q_p;
            double q_v;
            double q_a;

            Eigen::MatrixXd x, u, xd, ud, Ak, Bk;
        };
        MPCVar mpc_var;

        struct Model
        {
            int nx;
            int nu;
            double raduis;
            Eigen::ArrayXd x0, up;
        };
        Model model;

        struct Stage
        {
            Eigen::MatrixXd Qk;
            Eigen::MatrixXd Rk, Ak, Bk;
            Eigen::ArrayXd qfk, rfk, lb, ub;
        };

        std::array<Stage, N + 1> stages_;
        

        double begin_time, init_time, delta_t;

        Polynomial pn_x, pn_y, pn_z;
        

        void readJson();
        void getMPCMaterix();
        void quadprogInterface();
        

        public:
        mmpc(){};
        ~mmpc(){};
        Eigen::Array3d init(ros::NodeHandle &nh, polyCurve traj);
        void setBeginTime(const double reset_time);
        void getFeedback(const geometry_msgs::PoseStamped local_pos, 
                        const geometry_msgs::TwistStamped local_vel);
        Eigen::MatrixXd getReference();
        int getRuntime();
        Eigen::ArrayXd solver();

    };
}

#endif